﻿#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <math.h>


using namespace std;
using namespace Eigen;

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

static void demoAngleAxis()
{
    cout << "demoAngleAxis:" << endl;
    // yaw-z pitch-y roll-x,并转成弧度
    float yaw = 0 * M_PI / 180;
    float pitch = 0 * M_PI / 180;
    float roll = 45 * M_PI / 180;
    Matrix3f m;
    m = AngleAxisf(roll, Vector3f::UnitX()) *
            AngleAxisf(pitch, Vector3f::UnitY()) *
            AngleAxisf(yaw, Vector3f::UnitZ());
    cout << m << endl;
    cout << "is unitary:" << m.isUnitary() << endl;
    AngleAxisf aa;
    aa.fromRotationMatrix(m);
    cout << "aa:" << endl;
    cout << "axis:" << aa.axis() << " angle:" << aa.angle() << endl;
    cout << "R:" << aa.toRotationMatrix() << endl;

    // to euler angle Z-Y-X
    Eigen::Vector3f eulerAngle = m.eulerAngles(2, 1, 0);
    cout << "eulerAngle: " << eulerAngle.transpose() << endl;
}

static void demoQuaternion()
{
    AngleAxisf aaf1(0.1,Vector3f::UnitX());
    AngleAxisf aaf2(0.3,Vector3f::UnitY());
    Quaternionf qf1(aaf1),qf2(aaf2);
    // .coeffs():返回四元数的四个数(可修改)，可以对其进行索引[]获取值。需要注意的是返回顺序是x、y、z、w，和定义的时候是不一样的。
    cout<<qf1.coeffs()<<endl<<endl;
    cout<<qf2.coeffs()<<endl<<endl;

    Quaternionf inp_qf = qf1.slerp(0.4,qf2);
    cout<<inp_qf.coeffs()<<endl;
}

static void demoTranslation()
{
    Translation2d t1(1,4);
    Translation2d t2(2,7);
    Translation2d t3;
    t3=t1*t2;
    cout << "t1: " << t1.vector() << endl;
    cout << "t2: " << t2.translation() << endl;
    cout << "t3: " << t3.translation() <<endl;
}

static void demoMatrixHomo()
{
    AngleAxisd rotation(3.1415926 / 4, Vector3d(1, 0, 1).normalized());
    Vector3d translation(1, 3, 4);
    Isometry3d T1 = Isometry3d::Identity();
    Isometry3d T2 = Isometry3d::Identity();
    Isometry3d T3 = Isometry3d::Identity();

    T1.translate(translation);
    T1.rotate(rotation);

    // translation * T2
    T2.rotate(rotation);
    T2.pretranslate(translation);

    // T2 * translation
    T3.rotate(rotation);
    T3.translate(translation);

    cout << "rotation matrix:" << endl << rotation.toRotationMatrix() << endl << endl;
    cout << "translation vector:" << endl << translation << endl << endl;
    cout << "translation matrix1:" << endl << T1.matrix() << endl << endl;
    cout << "translation matrix1:" << endl << T2.matrix() << endl << endl;
    cout << "translation matrix1:" << endl << T3.matrix() << endl << endl;
}

int main(int argc, char *argv[])
{
    int option = 1;
    if (argc == 2) {
        option = atoi(argv[1]);
    }

    switch (option) {
    case 1: demoAngleAxis(); break;
    case 2: demoQuaternion(); break;
    case 3: demoTranslation(); break;
    case 4: demoMatrixHomo(); break;
    default: break;
    }

    return 0;
}
